/**
 *
 *
 */

#include <opencv/cv.h>
#include <opencv/highgui.h>

#include <viso_stereo.h>
#include <vector>
#include <string>

#define __SHOW_FEATURES__

using namespace std;

VisualOdometryStereo *viso = NULL;
VisualOdometryStereo::parameters viso_param;
vector<Matcher::p_match> viso_matches;
vector<int> viso_inliers_indices;

double focus = 0.0;
double image_center_x = 0.0;
double image_center_y = 0.0;
double baseline = 0.0;

vector<pair<string, string> > image_filenames;

void
convert_image_rgb_to_gray(unsigned char *lsrc, unsigned char *ldst, unsigned char *rsrc, unsigned char *rdst, int width, int height, int cutline)
{
	int i;
	int n = width * height;
	int step;

	for (i = 0; i < n; i++)
	{
		if (i > cutline * width)
		{
			step = 3 * i;

			ldst[i]= 0.30 * lsrc[step] + 0.59 * lsrc[step + 1] + 0.11 * lsrc[step + 2];
			rdst[i]= 0.30 * rsrc[step] + 0.59 * rsrc[step + 1] + 0.11 * rsrc[step + 2];
		}
		else
		{
			ldst[i] = 0;
			rdst[i] = 0;
		}
	}
}


void
visual_odometry_copy_inliers(int* vector_inliers, std::vector<Matcher::p_match> _matches, std::vector<int> _inliers)
{
	for (unsigned int i=0; i<_inliers.size(); i++)
	{
		vector_inliers[4*i] = _matches[_inliers[i]].u1p;
		vector_inliers[4*i+1] = _matches[_inliers[i]].v1p;
		vector_inliers[4*i+2]= _matches[_inliers[i]].u1c;
		vector_inliers[4*i+3]= _matches[_inliers[i]].v1c;
	}
}


void
detect_features_from_stereo_pair(IplImage *left, IplImage *right)
{
	IplImage *left_gray = cvCreateImage(cvSize(left->width, left->height), IPL_DEPTH_8U, 1);
	IplImage *right_gray = cvCreateImage(cvSize(right->width, right->height), IPL_DEPTH_8U, 1);

	cvCvtColor(left, left_gray, CV_BGR2GRAY);
	cvCvtColor(right, right_gray, CV_BGR2GRAY);

	int32_t dims[] = {left->width, left->height, left->width};

	if (viso->process((unsigned char *) left_gray->imageData, (unsigned char *) right_gray->imageData, dims))
	{
		if (viso->getNumberOfMatches() != 0)
		{
			viso_inliers_indices = viso->getInlierIndices();
			viso_matches = viso->getMatches();

			printf("%ld ", viso_inliers_indices.size());

			for (unsigned int i = 0; i < viso_inliers_indices.size(); i++)
			{
				double left_x = viso_matches[viso_inliers_indices[i]].u1c;
				double left_y = viso_matches[viso_inliers_indices[i]].v1c;

				double right_x = viso_matches[viso_inliers_indices[i]].u2c;
				double right_y = viso_matches[viso_inliers_indices[i]].v2c;

				printf("%d %d %d %d ", (int) left_x, (int) left_y, (int) right_x, (int) right_y);

#ifdef __SHOW_FEATURES__

				cvCircle(left, cvPoint(left_x, left_y), 2, cvScalar(0, 0, 255, 0), 1, 1, 0);
				cvCircle(right, cvPoint(right_x, right_y), 2, cvScalar(0, 0, 255, 0), 1, 1, 0);

#endif

			}

			printf("\n");

#ifdef __SHOW_FEATURES__

			cvShowImage("left", left);
			cvShowImage("right", right);
			cvWaitKey(-1);

#endif

		}
	}
	else
		fprintf(stderr, "FAIL!\n");

	cvReleaseImage(&left_gray);
	cvReleaseImage(&right_gray);
}


bool
visual_odometry_initialize(float focal_length, float principal_point_x, float principal_point_y, float baseline)
{
	viso_param.calib.f = focal_length;
	viso_param.calib.cu = principal_point_x;
	viso_param.calib.cv = principal_point_y;
	viso_param.base = baseline;

	viso_param.bucket.max_features = 4;
	viso_param.bucket.bucket_width = 64;
	viso_param.bucket.bucket_height = 48;

	viso_param.match.nms_n                  = 4;
	viso_param.match.nms_tau                = 50;
	viso_param.match.match_binsize          = 50;
	viso_param.match.match_radius           = 200;
	viso_param.match.match_disp_tolerance   = 1;
	viso_param.match.outlier_disp_tolerance = 8;
	viso_param.match.outlier_flow_tolerance = 8;
	viso_param.match.multi_stage            = 1;
	viso_param.match.half_resolution        = 1;
	viso_param.match.refinement             = 1;

	if (viso == NULL)
	{
		viso = new VisualOdometryStereo(viso_param);
		return true;
	}
	else
		return false;
}


void
read_image_list(char *image_list_filename)
{
	char *left_image_filename = (char *) calloc (1024, sizeof(char));
	char *right_image_filename = (char *) calloc (1024, sizeof(char));

	FILE *file = fopen(image_list_filename, "r");

	while (!feof(file))
	{
		fscanf(file, "\n%s %s\n", left_image_filename, right_image_filename);
		image_filenames.push_back(pair<string, string>(string(left_image_filename), string(right_image_filename)));
	}

	fclose(file);

	free(left_image_filename);
	free(right_image_filename);
}


int
parse_parameters(int argc, char **argv)
{
	if (argc < 6)
		return 0;

	focus = atof(argv[1]);
	image_center_x = atof(argv[2]);
	image_center_y = atof(argv[3]);
	baseline = atof(argv[4]);

	read_image_list(argv[5]);

	return 1;
}


void
show_usage_information_and_exit(int argc, char *argv[])
{
	printf("\n");
	printf("\nUse %s <foco-x> <centro-da-lente-x> <centro-da-lente-y> <baseline> <lista-imagens (um par stereo esquerda-direita por linha separado por espaco)>\n", argv[0]);
	printf("\n");
	printf("\nOutput: Para cada linha da entrada, a saida eh uma linha iniciada com o numero de features e para cada feature quatro inteiros representando as coordenadas x,y da feature na imagem esquerda e x,y da direita\n");
	printf("\n");


	exit(-1);
}


int
main(int argc, char **argv)
{
	if (parse_parameters(argc, argv) == 0)
		show_usage_information_and_exit(argc, argv);

	// initialize visual odometry with read parameters
	if (visual_odometry_initialize(focus, image_center_x, image_center_y, baseline))
	{
		for (unsigned int i = 0; i < image_filenames.size(); i++)
		{
			IplImage *left = cvLoadImage(image_filenames[i].first.c_str(), CV_LOAD_IMAGE_COLOR);
			IplImage *right = cvLoadImage(image_filenames[i].second.c_str(), CV_LOAD_IMAGE_COLOR);

			detect_features_from_stereo_pair(left, right);

			cvReleaseImage (&left);
			cvReleaseImage (&right);
		}
	}
	else
	{
		printf("Cannot start visual odometry module!\n");
		return (1);
	}
}

